#!/usr/bin/env python

import message_filters
import rospy
from geometry_msgs.msg import PoseStamped
import csv
from visualization_msgs.msg import Marker

PATH = "/home/hwk/catkin_ws/src/bag2csv/"


def callback(uav1, uav2, uav3, uav4, v_uav1, v_uav2, v_uav3, v_uav4, uav1_2, uav1_3, uav1_4, uav2_1, uav2_3, uav2_4,
             uav3_1, uav3_2, uav3_4, uav4_1, uav4_2, uav4_3):
    with open(PATH + 'uav.csv', 'a') as csvfile:
        uav1_writer = csv.writer(csvfile)
        uav1_writer.writerow([uav1.header.stamp, uav1.pose.orientation.x, uav1.pose.orientation.y,
                              uav1.pose.orientation.z, uav1.pose.orientation.w,
                              uav1_2.pose.position.x, uav1_2.pose.position.y, uav1_2.pose.position.z,
                              uav1_3.pose.position.x, uav1_3.pose.position.y, uav1_3.pose.position.z,
                              uav1_4.pose.position.x, uav1_4.pose.position.y, uav1_4.pose.position.z,

                              uav2.header.stamp, uav2.pose.orientation.x, uav2.pose.orientation.y,
                              uav2.pose.orientation.z, uav2.pose.orientation.w,
                              uav2_1.pose.position.x, uav2_1.pose.position.y, uav2_1.pose.position.z,
                              uav2_3.pose.position.x, uav2_3.pose.position.y, uav2_3.pose.position.z,
                              uav2_4.pose.position.x, uav2_4.pose.position.y, uav2_4.pose.position.z,

                              uav3.header.stamp, uav3.pose.orientation.x, uav3.pose.orientation.y,
                              uav3.pose.orientation.z, uav3.pose.orientation.w,
                              uav3_1.pose.position.x, uav3_1.pose.position.y, uav3_1.pose.position.z,
                              uav3_2.pose.position.x, uav3_2.pose.position.y, uav3_2.pose.position.z,
                              uav3_4.pose.position.x, uav3_4.pose.position.y, uav3_4.pose.position.z,

                              uav4.header.stamp, uav4.pose.orientation.x, uav4.pose.orientation.y,
                              uav4.pose.orientation.z, uav4.pose.orientation.w,
                              uav4_1.pose.position.x, uav4_1.pose.position.y, uav4_1.pose.position.z,
                              uav4_2.pose.position.x, uav4_2.pose.position.y, uav4_2.pose.position.z,
                              uav4_3.pose.position.x, uav4_3.pose.position.y, uav4_3.pose.position.z,

                              v_uav1.header.stamp, v_uav1.pose.position.x, v_uav1.pose.position.y,
                              v_uav1.pose.position.z,
                              v_uav1.pose.orientation.x, v_uav1.pose.orientation.y, v_uav1.pose.orientation.z,
                              v_uav1.pose.orientation.w,
                              v_uav2.header.stamp, v_uav2.pose.position.x, v_uav2.pose.position.y,
                              v_uav2.pose.position.z,
                              v_uav2.pose.orientation.x, v_uav2.pose.orientation.y, v_uav2.pose.orientation.z,
                              v_uav2.pose.orientation.w,
                              v_uav3.header.stamp, v_uav3.pose.position.x, v_uav3.pose.position.y,
                              v_uav3.pose.position.z,
                              v_uav3.pose.orientation.x, v_uav3.pose.orientation.y, v_uav3.pose.orientation.z,
                              v_uav3.pose.orientation.w,
                              v_uav4.header.stamp, v_uav4.pose.position.x, v_uav4.pose.position.y,
                              v_uav4.pose.position.z,
                              v_uav4.pose.orientation.x, v_uav4.pose.orientation.y, v_uav4.pose.orientation.z,
                              v_uav4.pose.orientation.w])




with open(PATH+'uav.csv', 'a') as csvfile:
    uav1_writer = csv.writer(csvfile, lineterminator='\n')
    uav1_writer.writerow(['uav1_stamp', 'uav1_qx', 'uav1_qy', 'uav1_qz', 'uav1_qw', 'uav1_2_x', 'uav1_2_y', 'uav1_2_z',
                          'uav1_3_x', 'uav1_3_y', 'uav1_3_z', 'uav1_4_x', 'uav1_4_y', 'uav1_4_z',
                          'uav2_stamp', 'uav2_qx', 'uav2_qy', 'uav2_qz', 'uav2_qw', 'uav2_1_x', 'uav2_1_y', 'uav2_1_z',
                          'uav2_3_x', 'uav2_3_y', 'uav2_3_z', 'uav2_4_x', 'uav2_4_y', 'uav2_4_z',
                          'uav3_stamp', 'uav3_qx', 'uav3_qy', 'uav3_qz', 'uav3_qw', 'uav3_1_x', 'uav3_1_y', 'uav3_1_z',
                          'uav3_2_x', 'uav3_2_y', 'uav3_2_z', 'uav3_4_x', 'uav3_4_y', 'uav3_4_z',
                          'uav4_stamp', 'uav4_qx', 'uav4_qy', 'uav4_qz', 'uav4_qw', 'uav4_1_x', 'uav4_1_y', 'uav4_1_z',
                          'uav4_2_x', 'uav4_2_y', 'uav4_2_z', 'uav4_3_x', 'uav4_3_y', 'uav4_3_z',
                          'vicon_uav1_stamp', 'vicon_uav1_x', 'vicon_uav1_y', 'vicon_uav1_z',
                          'vicon_uav1_qx', 'vicon_uav1_qy', 'vicon_uav1_qz', 'vicon_uav1_qw',
                          'vicon_uav2_stamp', 'vicon_uav2_x', 'vicon_uav2_y', 'vicon_uav2_z',
                          'vicon_uav2_qx', 'vicon_uav2_qy', 'vicon_uav2_qz', 'vicon_uav2_qw',
                          'vicon_uav3_stamp', 'vicon_uav3_x', 'vicon_uav3_y', 'vicon_uav3_z',
                          'vicon_uav3_qx', 'vicon_uav3_qy', 'vicon_uav3_qz', 'vicon_uav3_qw',
                          'vicon_uav4_stamp', 'vicon_uav4_x', 'vicon_uav4_y', 'vicon_uav4_z',
                          'vicon_uav4_qx', 'vicon_uav4_qy', 'vicon_uav4_qz', 'vicon_uav4_qw'])


rospy.init_node('reader', anonymous=True)
uav1_sub = message_filters.Subscriber("/uav1/mavros/local_position/pose", PoseStamped)
uav2_sub = message_filters.Subscriber("/uav2/mavros/local_position/pose", PoseStamped)
uav3_sub = message_filters.Subscriber("/uav3/mavros/local_position/pose", PoseStamped)
uav4_sub = message_filters.Subscriber("/uav4/mavros/local_position/pose", PoseStamped)

v_uav1_sub = message_filters.Subscriber("/UAV1/viconros/mocap/pos", PoseStamped)
v_uav2_sub = message_filters.Subscriber("/UAV2/viconros/mocap/pos", PoseStamped)
v_uav3_sub = message_filters.Subscriber("/UAV3/viconros/mocap/pos", PoseStamped)
v_uav4_sub = message_filters.Subscriber("/UAV4/viconros/mocap/pos", PoseStamped)

uav1_2_sub = message_filters.Subscriber("/Estimate_uav1_2", PoseStamped)
uav1_3_sub = message_filters.Subscriber("/Estimate_uav1_3", PoseStamped)
uav1_4_sub = message_filters.Subscriber("/Estimate_uav1_4", PoseStamped)
uav2_1_sub = message_filters.Subscriber("/Estimate_uav2_1", PoseStamped)
uav2_3_sub = message_filters.Subscriber("/Estimate_uav2_3", PoseStamped)
uav2_4_sub = message_filters.Subscriber("/Estimate_uav2_4", PoseStamped)
uav3_1_sub = message_filters.Subscriber("/Estimate_uav3_1", PoseStamped)
uav3_2_sub = message_filters.Subscriber("/Estimate_uav3_2", PoseStamped)
uav3_4_sub = message_filters.Subscriber("/Estimate_uav3_4", PoseStamped)
uav4_1_sub = message_filters.Subscriber("/Estimate_uav4_1", PoseStamped)
uav4_2_sub = message_filters.Subscriber("/Estimate_uav4_2", PoseStamped)
uav4_3_sub = message_filters.Subscriber("/Estimate_uav4_3", PoseStamped)

ts = message_filters.ApproximateTimeSynchronizer([uav1_sub, uav2_sub, uav3_sub, uav4_sub,
                                                  v_uav1_sub, v_uav2_sub, v_uav3_sub, v_uav4_sub,
                                                  uav1_2_sub, uav1_3_sub, uav1_4_sub, uav2_1_sub, uav2_3_sub,
                                                  uav2_4_sub, uav3_1_sub, uav3_2_sub, uav3_4_sub, uav4_1_sub,
                                                  uav4_2_sub, uav4_3_sub], 50, 0.1)
ts.registerCallback(callback)

rospy.spin()